#include <pinocchio/fwd.hpp>
#include <ocs2_oc/oc_problem/OptimalControlProblem.h>
#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/aba.hpp>

#include <string>
#include <iostream>

#include <ros/ros.h>
#include <ros/package.h>

#include "cartpole_dynamic.h"
using namespace ocs2;
using namespace cartpole;
int main()
{
    std::string package_path = ros::package::getPath("cartpole_gazebo_ros");
    std::cout << package_path << std::endl;
    std::string cartpole_urdf = package_path + std::string("/model/cartpole_pinocchio.urdf");
    std::string config_file = package_path + std::string("/config/config.info");
    std::string lib_path = package_path + std::string("/auto_generator");

    // 模型 使用 pinocchio
    pinocchio::Model cartpole_model;
    pinocchio::urdf::buildModel(cartpole_urdf, cartpole_model);
    PinocchioInterface pinocchioInterface(cartpole_model);
    auto &model = pinocchioInterface.getModel();
    auto &data = pinocchioInterface.getData();

    CartpoleDynamic dynamic(pinocchioInterface);
    CartpoleDynamic dynamic1(dynamic);
    scalar_t t = 0;
    vector_t x = vector_t(4).setZero();
    vector_t u = vector_t(1).setZero();
    PreComputation pc;
    x << 1, 0, 0, 0;
    u << 0.2;
    auto dx = dynamic.computeFlowMap(t, x, u, pc);
    auto dx1 = dynamic1.computeFlowMap(t, x, u, pc);
    std::cout << dx <<std::endl;
    std::cout << dx1<<std::endl;
    vector_t q(2);
    vector_t dq(2);
    q << 0, 1;
    dq << 0, 0;
    vector_t tau(2);
    tau << 0.2, 0;
    auto ddq = pinocchio::aba(model, data, q, dq, tau);
    vector_t ddq_from_dx(2);
    ddq_from_dx << dx[3], dx[2];
    std::cout << ddq_from_dx <<std::endl;
    std::cout << ddq << std::endl;
}